#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv/highgui.h>
#include "opencv2/gpu/gpu.hpp"
#include <koala_hog/bbox.h>
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <geometry_msgs/Twist.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <message_filters/subscriber.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/Vector3Stamped.h>
#include "opencv2/gpu/gpu.hpp"
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <message_filters/sync_policies/approximate_time.h>
#include <math.h>
#include <ctime>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/Int32.h>

namespace enc = sensor_msgs::image_encodings;
static const char WINDOW[] = "Image window";

ros::Publisher bounding_box_pub_;
ros::Publisher goal_pub;
ros::Publisher koala_twist_pub;

int colours_saver[6]={0};
bool isTracking = false;
time_t time_last_tracked = time(0);
double last_secs;
float fx = 525.0;
float fy = 525.0;
float cy = 319.5;
float cx = 239.5;
float ds = 1.0;


int frameCounter =0;


int* colour(cv::Mat image, cv::Mat image_depth, int* colours)
{
  int center_torso_x = image.cols/2;
  int center_torso_y = image.rows/2.9;

  float distance = image_depth.at<float>(center_torso_y, center_torso_x);
  if(!std::isnan(distance))
  {
      int count = 0;
      colours[0] = -1;
      colours[1] = -1;
      colours[2] = -1;
      colours[3] = -1;
      colours[4] = -1;
      colours[5] = -1;

      int center_legs_x = image.cols/2;
      int center_legs_y = image.rows/1.95;

      int thickness = -1;
      int lineType = 8;

      int R_torso =0;
      int G_torso =0;
      int B_torso =0;

      int R_leg =0;
      int G_leg =0;
      int B_leg =0;

      for(int i = center_torso_x-5; i<center_torso_x+5;i++)
      {
          for (int s = center_torso_y-5; s < center_torso_y+5; ++s) {
              cv::Vec3b bgrPixel = image.at<cv::Vec3b>(s, i);
              R_torso += (int)bgrPixel[2];
              G_torso += (int)bgrPixel[1];
              B_torso += (int)bgrPixel[0];
          }
      }

      for(int i = center_torso_x-100; i<center_torso_x+100;i++)
      {
          for (int j = center_legs_y+45; j < center_legs_y+70; j++) {
              float d = image_depth.at<float>(j, i);
              if(distance-0.2 <= d && distance+0.2 >=d && !std::isnan(d))
              {
                  cv::Vec3b bgrlPixel = image.at<cv::Vec3b>(j, i);
                  R_leg +=(int)bgrlPixel[2];
                  G_leg +=(int)bgrlPixel[1];
                  B_leg +=(int)bgrlPixel[0];
                  count++;
              }
          }
      }

      if(count==0)
      {
          colours[0] = -1;
          colours[1] = -1;
          colours[2] = -1;
          colours[3] = -1;
          colours[4] = -1;
          colours[5] = -1;
          return colours;
      }

      R_leg = (R_leg/(int)count);
      G_leg = (G_leg/(int)count);
      B_leg = (B_leg/(int)count);

      R_torso = (R_torso/100);
      G_torso = (G_torso/100);
      B_torso = (B_torso/100);

      std::cout << "LEGS R: " << (R_leg) << " G: " << (G_leg) << " B: " << (B_leg) << std::endl;
      //std::cout << "TORSO R: " << (R_torso) << " G: " << (G_torso) << " B: " << (B_torso) << std::endl;

          cv::circle( image,
                  cv::Point(center_torso_x,center_torso_y),
                  10,
                  cv::Scalar( 0, 0, 255 ),
                  thickness,
                  lineType );

          cv::circle( image,
                  cv::Point(center_legs_x,center_legs_y+35),
                  10,
                  cv::Scalar( 0, 0, 255 ),
                  thickness,
                  lineType );
      cv::imshow("CROPPED", image);

      //int colours[6];
      colours[0] = R_leg;
      colours[1] = G_leg;
      colours[2] = B_leg;
      colours[3] = R_torso;
      colours[4] = G_torso;
      colours[5] = B_torso;

      return colours;

  }

  colours[0] = -1;
  colours[1] = -1;
  colours[2] = -1;
  colours[3] = -1;
  colours[4] = -1;
  colours[5] = -1;
  return colours;
}

bool rgb_in_range(int* array)
{
    bool c1 = ( (colours_saver[0]-20) <= array[0] && array[0] <= (colours_saver[0]+20) );
    bool c2 = ( (colours_saver[1]-20) <= array[1] && array[1] <= (colours_saver[1]+20) );
    bool c3 = ( (colours_saver[2]-20) <= array[2] && array[2] <= (colours_saver[2]+20) );
    bool c4 = ( (colours_saver[3]-20) <= array[3] && array[3] <= (colours_saver[3]+20) );
    bool c5 = ( (colours_saver[4]-20) <= array[4] && array[4] <= (colours_saver[4]+20) );
    bool c6 = ( (colours_saver[5]-20) <= array[5] && array[5] <= (colours_saver[5]+20) );

    if(c1&&c2&&c3&&c4&&c5&&c6)
    {
        return true;
    }
    return false;
}


void koala_move(std_msgs::Int32 angle, std_msgs::Int32 distance)
{
    geometry_msgs::Twist koala_drive;
    koala_drive.linear.x = distance.data;
    koala_drive.angular.z = angle.data;
    koala_twist_pub.publish(koala_drive);
    //sleep(1);
}

void pedestrian_tracker(const koala_hog::bbox &bounding_box, cv::Mat &depth_image)
{
    int center_bounding_box_x = bounding_box.x+0.5*bounding_box.width;
    int center_bounding_box_y = bounding_box.y+0.5*bounding_box.height;

    float distance = ds * depth_image.at<float>(center_bounding_box_y, center_bounding_box_x);
    float real_world_x = (center_bounding_box_x - cx) * distance / fx;
    float real_world_y = (center_bounding_box_y - cy) * distance / fy;
    float area = bounding_box.height * bounding_box.width;


        if(!std::isnan(distance) && !std::isnan(real_world_x) && !std::isnan(real_world_y))// && !real_min_y > 0.4)
        {
            if(real_world_y > -0.2 && real_world_y < 0.4)// && area >= 130000-(30000*(distance-1.8)) && distance < 6.5)
            {
                std::cout << "X: " << real_world_x << ", Y: " << real_world_y << ", Z: " << distance << std::endl;

                // Calculate angle
                float angle = asin(real_world_x/distance) * 57.2957795;
                float travel_distance = distance - 1.5;

                // setup ROS services
                std_msgs::Int32 pub_angle;
                std_msgs::Int32 pub_distance;

                pub_angle.data = angle * 0.85;
                pub_distance.data = (travel_distance * 100);

                //srv_angle.request.req = angle * 0.85;
                //srv_distance.request.req = (travel_distance * 100);
                std::cout << "ANGLE: " << angle << std::endl;

                // get RGB values
                int tempRGB[6];
                tempRGB[0] = bounding_box.rl;
                tempRGB[1] = bounding_box.gl;
                tempRGB[2] = bounding_box.bl;
                tempRGB[3] = bounding_box.rt;
                tempRGB[4] = bounding_box.gt;
                tempRGB[5] = bounding_box.bt;

                if(isTracking)
                {
                    if(rgb_in_range(tempRGB))
                    {
                        std::cout << "IS TRACKING COLOURS OK" <<std::endl;
                        //koala_move(srv_distance, srv_angle);
                        koala_move(pub_angle, pub_distance);
                        for (int i = 0; i < 6; ++i)
                        {
                            colours_saver[i] = tempRGB[i];
                        }
                        //koala_move(srv_distance, srv_angle);
                        geometry_msgs::Vector3 goal;
                        goal.x = real_world_x;
                        goal.y = real_world_y;
                        goal.z = distance;
                        goal_pub.publish(goal);
                        last_secs = ros::Time::now().toSec();
                        frameCounter=0;
                    }
                    else if (ros::Time::now().toSec()-last_secs > 10)
                    {
                        std::cout << "WAS TRACKING, BUT AFTER TIMEOUT STARTING TRACKING NEW TARGET" <<std::endl;
                        if(angle <= 5.0 && distance < 4.0)
                        {
                            for (int i = 0; i < 6; ++i)
                            {
                                colours_saver[i] = tempRGB[i];
                            }
                            koala_move(pub_angle, pub_distance);
    //                        //koala_move(srv_distance, srv_angle);
    //                        geometry_msgs::Vector3 goal;
    //                        goal.x = real_world_x;
    //                        goal.y = real_world_y;
    //                        goal.z = distance;
    //                        goal_pub.publish(goal);
                            last_secs = ros::Time::now().toSec();
                            isTracking = true;
                            frameCounter=0;
                        }
                        else
                        {
                            std::cout<< "STOPPED TRACKING AFTER TIMEOUT" <<std::endl;
                            frameCounter=0;
                            pub_angle.data=0;
                            pub_distance.data=0;
                            koala_move(pub_angle, pub_distance);
                            isTracking = false;
                        }
                    }
                    else if(ros::Time::now().toSec()-last_secs < 10)
                    {
                        std::cout << "TRYING TRACKING, BUT NO TARGET SEEN BUT NO TIMEOUT YET: " << ros::Time::now().toSec()-last_secs <<std::endl;
                        frameCounter++;
                        if(frameCounter>=30)
                        {
                        pub_angle.data=0;
                        pub_distance.data=0;
                        koala_move(pub_angle, pub_distance);
                        }
                    }
                    else
                    {
                        std::cout<<"TRACKING BUT DOING NOTHING, time " << ros::Time::now().toSec()-last_secs << std::endl;
                        frameCounter++;
                        if(frameCounter>=30)
                        {
                        pub_angle.data=0;
                        pub_distance.data=0;
                        koala_move(pub_angle, pub_distance);
                        }
                    }
                }
                else
                {
                    if(angle <= 5.0 && distance < 4.0)
                    {
                        std::cout<< "TRACKING NEW TARGET" << std::endl;
                        for (int i = 0; i < 6; ++i)
                        {
                            colours_saver[i] = tempRGB[i];
                        }
                        //koala_move(srv_distance, srv_angle);
                        frameCounter=0;
                        koala_move(pub_angle, pub_distance);
                        geometry_msgs::Vector3 goal;
                        goal.x = real_world_x;
                        goal.y = real_world_y;
                        goal.z = distance;
                        goal_pub.publish(goal);
                        last_secs = ros::Time::now().toSec();
                        isTracking = true;
                    }
                }
            }
        }
    }

void pedestrian_detector(koala_hog::bbox &output, cv::Mat & rotated, cv::Mat &depth_image)
{
    cv::Mat show;
    // Initialize GPU Mats
    cv::gpu::GpuMat src_gpu, mono_gpu, convertedImg;

    // Initialize HOG classifier and set descriptor class
    cv::gpu::HOGDescriptor hog;
    hog.setSVMDetector(cv::gpu::HOGDescriptor::getDefaultPeopleDetector());

    // Create vector of bounding boxes
    std::vector<cv::Rect> found;

    // Convert FUERTE kinect image to ELECTRIC OpenCV GPU image
    //src_gpu.upload(cv_ptr->image);
    src_gpu.upload(rotated);
    cv::gpu::cvtColor(src_gpu, convertedImg, CV_BGR2RGB);
    cv::gpu::cvtColor(convertedImg, mono_gpu, CV_RGB2GRAY);
    convertedImg.download(show);

    // Detect pedestrains
    hog.detectMultiScale(mono_gpu, found);

    // Draw detected pedestrians in image
    for(unsigned i = 0; i < found.size(); i++) {
        cv::Rect r = found[i];
        if(r.height > r.width && r.area()>1000)
        {
            cv::Mat croppedImage = show(r);
            cv::Mat croppedImage_depth = depth_image(r);
            int rgb[6];// = colour(croppedImage);
            colour(croppedImage, croppedImage_depth, rgb);
            std::cout<<rgb[0]<<std::endl;
            if(rgb[0]>-1)
            {
                koala_hog::bbox output;

                output.rl = rgb[0];
                output.gl = rgb[1];
                output.bl = rgb[2];
                output.rt = rgb[3];
                output.gt = rgb[4];
                output.bt = rgb[5];
                //output.header = msg->header;
                output.x = r.x;
                output.y = r.y;
                output.height = r.height;
                output.width = r.width;
                cv::rectangle(show, r.tl(), r.br(), cv::Scalar(0,255,0), 2);
                pedestrian_tracker(output,depth_image);
                bounding_box_pub_.publish(output);
            }
        }
    }

    cv::imshow(WINDOW,show);
    cv::waitKey(3);
}

cv::Mat ros_to_cv_color(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    cv::Mat rotated;

    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, enc::RGB8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return rotated;
    }

    cv::transpose(cv_ptr->image, rotated);
    cv::flip(rotated, rotated, -1);
    cv::flip(rotated, rotated, 1);

    return rotated;
}

cv::Mat ros_to_cv_depth(const sensor_msgs::ImageConstPtr& msg)
{
    cv::Mat depth_image;
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return depth_image;
    }

    cv::transpose(cv_ptr->image, depth_image);
    cv::flip(depth_image, depth_image, -1);
    cv::flip(depth_image, depth_image, 1);

    return depth_image;
}

void callback(const sensor_msgs::ImageConstPtr& image_depth, const sensor_msgs::ImageConstPtr& image_color)
{
    cv::Mat color_image = ros_to_cv_color(image_color);
    cv::Mat depth_image = ros_to_cv_depth(image_depth);
    koala_hog::bbox box;
    pedestrian_detector(box, color_image, depth_image);
}

int main(int argc, char** argv)
{
  cv::namedWindow(WINDOW, CV_WINDOW_NORMAL);

  ros::init(argc, argv, "koala_follower");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);

  bounding_box_pub_ = nh.advertise<koala_hog::bbox>("hog_bounding_box", 1);
  koala_twist_pub = nh.advertise<geometry_msgs::Twist>("/koala/twist", 100);
  last_secs = ros::Time::now().toSec();

  goal_pub = nh.advertise<geometry_msgs::Vector3>("/koala/goal_pos", 100);

  image_transport::SubscriberFilter image_depth(it, "/camera/decompressedDepth", 1);
  image_transport::SubscriberFilter image_color(it, "/camera/decompressedColor", 1);


  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(1000000000), image_depth, image_color);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}
